//============================================================================
// Name        : csiena
// Author      : Josh Lospinoso
// Copyright   : MIT License
// Description : Austere, limited implementation of the R package RSiena in
//				 C++. Used for fast maximum likelihood and method of moments
//				 estimation of stochastic actor oriented models (SAOMs)
//				 without any frills or user experience niceties.
//============================================================================
#include "mcmcmle.h"
namespace csiena {

mcmcmle::mcmcmle(int actors, int seed) {
	this->actors = actors;
	this->r = new MersenneTwister(seed);
	this->completeData = new Chain(actors);
	this->betaScore = new double[BETA_LENGTH];
	this->convergenceStatistics = new double[THETA_LENGTH];
	this->betaInformation = new double[BETA_LENGTH*BETA_LENGTH];
	this->alphaInformation = 0;
	this->alphaScore = 0;
	#if GAMMA_LENGTH > 0
		this->gammaScore = new double[GAMMA_LENGTH];
		this->gammaInformation = new double[GAMMA_LENGTH*GAMMA_LENGTH];
	#endif
	this->scoreSquaredSum = new double[THETA_LENGTH*THETA_LENGTH];
	this->derivativeMatrix = new double[THETA_LENGTH*THETA_LENGTH];
	this->standardErrorsSquared = new double[THETA_LENGTH*THETA_LENGTH];
	this->subphaseSumTheta = new double[THETA_LENGTH];
	this->subphasePreviousScore = new double[THETA_LENGTH];
	this->subphaseAutocorrScore = new double[THETA_LENGTH];
	#ifndef CUDA
	matrixGSL = gsl_matrix_alloc(THETA_LENGTH,THETA_LENGTH);
	matrixInverseGSL = gsl_matrix_alloc(THETA_LENGTH,THETA_LENGTH);
	luPermutationGSL = gsl_permutation_alloc(THETA_LENGTH);
	#ifdef BOOST_THREADS
		for (int i=0; i<BOOST_THREADS; i++) {
			this->threadR[i] = new MersenneTwister(this->r->getSeed());
			this->results[i] = new EstimationResults();
		}
	#endif
	#else
	// TODO: This should be a CUDA enabled inversion
	this->status = culaInitialize();
	if(this->status != culaNoError) {
		std::cerr << "CULA Initialization error: " << culaGetStatusString(this->status) << std::endl;
		std::cerr << "Error code " << culaGetErrorInfo() << ": " << cudaGetErrorString(cudaGetLastError())  << std::endl;
	}

	#endif

	this->resetLikelihoodStatistics(false);
}

mcmcmle::~mcmcmle() {
	delete(this->completeData);
	delete(this->betaScore);
	delete(this->betaInformation);
	#if GAMMA_LENGTH > 0
	delete(this->gammaScore);
	delete(this->gammaInformation);
	#endif
	delete(this->scoreSquaredSum);
	delete(this->derivativeMatrix);
	delete(this->standardErrorsSquared);
	#ifndef CUDA
	gsl_matrix_free(matrixInverseGSL);
	gsl_matrix_free(matrixGSL);
	gsl_permutation_free(luPermutationGSL);
		#ifdef BOOST_THREADS
			for (int i=0; i<BOOST_THREADS; i++) {
				free(threadR[i]);
				free(results[i]);
			}
		#endif
	#else
	culaShutdown();
	#endif
	delete(this->convergenceStatistics);
	delete(this->subphaseSumTheta);
	delete(this->subphasePreviousScore);
	delete(this->subphaseAutocorrScore);
}

void mcmcmle::setBeta(int i, double beta) {
	if (i < BETA_LENGTH) {
		this->completeData->linkEvaluationFunction->setBeta(i, beta);
	} else {
		std::cerr << "mcmcmle::setBeta: index out of bounds " << i << std::endl;
	}
}

void mcmcmle::setGamma(int i, double gamma) {
	if (i < GAMMA_LENGTH) {
		this->completeData->crimpEvaluationFunction->setGamma(i,gamma);
	} else {
		std::cerr << "mcmcmle::setGamma: index out of bounds " << i << std::endl;
	}
}

double mcmcmle::getAlpha() {
	return this->completeData->getAlpha();
}

void mcmcmle::setAlpha(double value) {
	this->completeData->setAlpha(value);
}

double mcmcmle::getBeta(int i) {
	if (i < BETA_LENGTH) {
		return this->completeData->linkEvaluationFunction->getBeta(i);
	} else {
		std::cerr << "mcmcmle::getBeta: index out of bounds " << i << std::endl;
		return 0.0;
	}
}

double mcmcmle::getGamma(int i) {
	if (i < GAMMA_LENGTH) {
		return this->completeData->crimpEvaluationFunction->getGamma(i);
	} else {
		std::cerr << "mcmcmle::getGamma: index out of bounds " << i << std::endl;
		return 0.0;
	}
}

void mcmcmle::generateSimulatedChain(int chainSize) {
	this->generateSimulatedChain(chainSize,0,2);
}

void mcmcmle::generateSimulatedChain(int chainSize, int burnInsForStart, int observations) {
	this->completeData->resetChain();
	this->completeData->simulateStartingDigraph(burnInsForStart,this->r);
	this->completeData->insertSimulatedCrimpAfter(this->r);
	for (int crimp = 1; crimp < observations; crimp++) {
		for (int chainUpdate=0; chainUpdate < chainSize; chainUpdate++) {
				this->completeData->insertSimulatedLinkAfter(this->r);
		}
		this->completeData->insertSimulatedCrimpAfter(this->r);
	}
	this->completeData->evaluateChain();
}

void mcmcmle::estimateMCMC(int sampleSpacing, int samples, bool estimateInformation) {
	#ifdef BOOST_THREADS
		//TODO: Debug BOOST threads for new estimateMCMC (i.e. Gamma/Alpha)
		int samplesPerThread = ceil((double)samples / BOOST_THREADS);
		samples = samplesPerThread * BOOST_THREADS;
		std::cout << " **MultiThread: Taking " << samples << " = " << samplesPerThread << " x " << BOOST_THREADS << " samples." << std::endl;
		for (int i=0; i<BOOST_THREADS; i++) {
			this->threadChains[i] = new Chain(*(this->completeData));
			if (i == BOOST_THREADS-1) {
				this->workers[i] = new EstimationWorker(this->completeData , threadR[i]);
			} else {
				this->workers[i] = new EstimationWorker(threadChains[i], threadR[i]);
			}
			this->workerThreads[i] = new boost::thread(&EstimationWorker::run, *(this->workers[i]),
					this->results[i], sampleSpacing, samplesPerThread);
		}
		for (int i=0; i<BOOST_THREADS; i++) {
			this->workerThreads[i]->join();
		}
		this->resetLikelihoodStatistics(!estimateInformation);
		this->sampleSize=samples;
		for (int i=0; i<BOOST_THREADS; i++) {
			this->mcmcProposalsAcceptedInPhase += this->results[i]->mcmcProposalsAccepted;
			this->mcmcProposalsMadeInPhase += this->results[i]->mcmcProposalsMade;
			this->incrementLogLikelihoodSum( this->results[i]->logLikelihoodSum );
			for (int k=0; k < BETA_LENGTH; k++) {
				this->incrementBetaScoreSum(k, this->results[i]->scoresSum[k]);
				for (int l=0; l<BETA_LENGTH; l++) {
					this->incrementBetaInformationSum(k,l, this->results[i]->informationSum[k*BETA_LENGTH+l]);
					this->incrementScoreSquaredSum(k,l, this->results[i]->scoresSquaredSum[k*BETA_LENGTH+l]);
				}
			}
		}
		for (int i=0; i<BOOST_THREADS; i++) {
			delete(this->threadChains[i]);
			delete(this->workers[i]);
			delete(this->workerThreads[i]);
		}
	#else
		this->resetLikelihoodStatistics(!estimateInformation);
		this->sampleSize=samples;
		this->chainLengthSum = 0;
		for (int draw=0; draw < samples * (sampleSpacing+1); draw++) {
			this->mcmcProposalsMadeInPhase++;
			if (this->completeData->drawProposal(r)) {
				this->mcmcProposalsAcceptedInPhase++;
			}
			if (draw % (sampleSpacing+1) == 0) {
				this->completeData->evaluateChain();
				this->incrementAlphaScoreSum(this->completeData->getAlphaScore());
				this->incrementAlphaInformationSum(this->completeData->getAlphaInformation());
				// Draw sample
				this->chainLengthSum += this->completeData->getChainSize() - this->completeData->getCrimpCount();
				this->incrementLogLikelihoodSum(this->completeData->getLogLikelihood());
				for (int k=0; k < THETA_LENGTH; k++) {
					this->incrementScoreSum(k, this->completeData->getScore(k));
					for (int l=0; l<THETA_LENGTH; l++) {
						this->incrementInformationSum(k,l, this->completeData->getInformation(k, l));
						this->incrementScoreSquaredSum(k,l,this->completeData->getScore(k)*this->completeData->getScore(l));
					}
				}
			}
		}
	#endif /* BOOST_THREADS */
	if (estimateInformation) {
		this->calculateDerivativeMatrix();
	}
}

void mcmcmle::estimateCompleteData() {
	// Here, we find the mean log likelihood, scores, and information evaluated at the current beta
	this->resetLikelihoodStatistics(false);
	this->completeData->evaluateChain();
	this->setLogLikelihoodSum(this->completeData->getLogLikelihood());
	for (int i=0; i<BETA_LENGTH; i++) {
		this->setBetaScoreSum(i, this->completeData->getBetaScore(i));
		for (int j=0; j<BETA_LENGTH; j++) {
			this->setBetaInformationSum(i,j, this->completeData->getBetaInformation(i,j));
			this->setScoreSquaredSum(i,j,this->completeData->getBetaScore(i)*this->completeData->getBetaScore(j));
		}
	}
	for (int k=0; k < GAMMA_LENGTH; k++) {
		this->incrementGammaScoreSum(k, this->completeData->getGammaScore(k));
		for (int l=0; l<GAMMA_LENGTH; l++) {
			this->incrementGammaInformationSum(k,l, this->completeData->getGammaInformation(k, l));
			this->incrementScoreSquaredSum(k+BETA_LENGTH,l+BETA_LENGTH,
					this->completeData->getGammaScore(k)*this->completeData->getGammaScore(l));
		}
	}
	this->sampleSize = 1;
	this->calculateDerivativeMatrix(true,false);
	this->calculateStandardErrorsSquared();
}

double mcmcmle::getLogLikelihood() {
	return this->completeData->getLogLikelihood() / (double)this->sampleSize;
}

void mcmcmle::burnChain(int draws) {
	// Burn a few permutations
	for (int draw=0; draw < draws; draw++) {
		if (this->completeData->drawProposal(this->r)) {
			this->mcmcProposalsAcceptedInPhase++;
		}
		this->mcmcProposalsMadeInPhase++;
	}
}

void mcmcmle::loadStart(const char* fname) {
	this->completeData->resetChain();
	this->completeData->startingDigraph->readNet(fname);
	this->completeData->resetX();
	this->completeData->insertCrimpAfter(fname);
}

void mcmcmle::loadObservation(const char* fname) {
	if (this->completeData->currentElement) {
		while(this->completeData->currentElement->isLink()) {
			this->completeData->retreatChain();
		}
		Digraph end(this->actors);
		end.readNet(fname);
		int hammingDistance = 0;
		double logFalsePositiveProbability = -99999999;
		if (GAMMA_LENGTH==2) {
			if (this->completeData->crimpEvaluationFunction->getGamma(1) != 0) {
				logFalsePositiveProbability = log(this->completeData->crimpEvaluationFunction->getGamma(1));
			}
		} else if (GAMMA_LENGTH==1) {
			if (this->completeData->crimpEvaluationFunction->getGamma(0) != 0) {
				logFalsePositiveProbability = log(this->completeData->crimpEvaluationFunction->getGamma(0));
			}
		}
		for (int i=0; i<this->actors; i++) {
			for (int j=0; j<this->actors; j++) {
				if (end.get(i,j) != this->completeData->currentDigraph->get(i,j)) {
					hammingDistance++;
				}
			}
		}
		while (--hammingDistance >= 0) {
			// Evaluate the most likely link to enter
			double maxChoiceProb = -99999999;
			int iMax = -1;
			int jMax = -1;
			for (int i=0; i<this->actors; i++) {
				for (int j=0; j<this->actors; j++) {
					if (end.get(i,j) != this->completeData->currentDigraph->get(i,j)) {
						ChainLink temporary(this->actors);
						temporary.i = i;
						temporary.j = j;
						double temporaryChoiceProb = temporary.evaluateProposal(this->completeData->currentDigraph, this->completeData->linkEvaluationFunction);
						if ( temporaryChoiceProb > maxChoiceProb) {
							iMax = i;
							jMax = j;
							maxChoiceProb = temporaryChoiceProb;
						}
					}
				}
			}
			if ((GAMMA_LENGTH > 0) & (maxChoiceProb < logFalsePositiveProbability)) {
				// Do nothing, make it an observation error
			} else {
				this->completeData->insertLinkAfter(iMax,jMax);
			}
		}
		this->completeData->insertCrimpAfter(fname);
	} else {
		this->loadStart(fname);
	}
}

double mcmcmle::getBetaScore(int i) {
	if ((i < BETA_LENGTH) & (this->sampleSize > 0)) {
		return this->betaScore[i] / (double)this->sampleSize;
	} else {
		std::cerr << "mcmcmle::getBetaScore: index = " << i << " and sample size = " << (double)this->sampleSize << std::endl;
		return 0.0;
	}
}

double mcmcmle::getGammaScore(int i) {
	if ((i < GAMMA_LENGTH) & (this->sampleSize > 0)) {
		return this->gammaScore[i] / (double)this->sampleSize;
	} else {
		std::cerr << "mcmcmle::getGammaScore: index = " << i << " and sample size = " << (double)this->sampleSize << std::endl;
		return 0.0;
	}
}

double mcmcmle::getBetaInformation(int j, int k) {
	if ((j < BETA_LENGTH) & (k < BETA_LENGTH) & (this->sampleSize > 0)) {
		return this->betaInformation[j * BETA_LENGTH + k] / (double)this->sampleSize;
	} else {
		std::cerr << "mcmcmle::getBetaInformation: index = " << j << "," << k << " and sample size = " << (double)this->sampleSize << std::endl;
		return 0.0;
	}
}

double mcmcmle::getGammaInformation(int j, int k) {
	if ((j < GAMMA_LENGTH) & (k < GAMMA_LENGTH) & (this->sampleSize > 0)) {
		return this->gammaInformation[j * GAMMA_LENGTH + k] / (double)this->sampleSize;
	} else {
		std::cerr << "mcmcmle::getGammaInformation: index = " << j << "," << k << " and sample size = " << (double)this->sampleSize << std::endl;
		return 0.0;
	}
}

double mcmcmle::getObservedInformation(int j, int k) {
	return this->getInformation(j,k) - this->getScoreCovariance(j,k);
}

double mcmcmle::getScoreCovariance(int j, int k) {
	if ((j < THETA_LENGTH) & (k < THETA_LENGTH) & (this->sampleSize > 0)) {
		double returnValue = 0.0;
		if (this->sampleSize > 1) {
			returnValue = (this->getScoreSquaredSum(j,k) - (double)this->sampleSize*this->getScore(j)*this->getScore(k))
					/ ((double)this->sampleSize - 1);
		}
		return returnValue;
	} else {
		std::cerr << "mcmcmle::getScoreCovariance: index = " << j << "," << k << " and sample size = " << (double)this->sampleSize << std::endl;
		return 0.0;
	}
}

void mcmcmle::calculateStandardErrorsSquared() {
	#ifndef CUDA
	for (int i=0; i<THETA_LENGTH; i++) {
		for (int j=0; j<THETA_LENGTH; j++) {
			gsl_matrix_set(matrixGSL, i, j, this->getObservedInformation(i,j));
		}
	}
	int s;
	gsl_linalg_LU_decomp(matrixGSL, luPermutationGSL, &s);
	gsl_linalg_LU_invert(matrixGSL, luPermutationGSL, matrixInverseGSL);
	for (int i=0; i<THETA_LENGTH; i++) {
		for (int j=0; j<THETA_LENGTH; j++) {
			this->setStandardErrorSquared(i,j, gsl_matrix_get(matrixInverseGSL,i,j));
		}
	}
	#else
	// TODO: This should be a CUDA enabled inversion, since gsl gnu is not compatible with nvcc.
	for (int i=0; i<THETA_LENGTH; i++) {
		for (int j=0; j<THETA_LENGTH; j++) {
			if (i != j) {
				this->setStandardErrorSquared(i,j, 0);
			} else {
				this->setStandardErrorSquared(i,i, 1/this->getObservedInformation(i,i));
			}
		}
	}
	#endif
}

void mcmcmle::calculateDerivativeMatrix() {
	// Default to using the RSiena defaults
	this->calculateDerivativeMatrix(true, true);
}

double mcmcmle::getScore(int k) {
	double value = 0.0;
	if (k < BETA_LENGTH) {
		value = this->getBetaScore(k);
	} else if (k < THETA_LENGTH) {
		value = this->getGammaScore(k-BETA_LENGTH);
	} else {
		std::cerr << "mcmcmle::getScore Error: " << k << " out of bounds for score function." << std::endl;
	}
	return value;
}

double mcmcmle::getInformation(int k, int l) {
	double value = 0.0;
	if ((k < BETA_LENGTH) & (l < BETA_LENGTH)) {
		value = this->getBetaInformation(k,l);
	} else if ((k < THETA_LENGTH) & (k >= BETA_LENGTH) & (l < THETA_LENGTH) & (l >= BETA_LENGTH)) {
		value = this->getGammaInformation(k-BETA_LENGTH, l-BETA_LENGTH);
	} else if ((k >= THETA_LENGTH) | (l >= THETA_LENGTH)) {
		std::cerr << "mcmcmle::getInformation Error: " << k << "," << l << " out of bounds." << std::endl;
	}
	return value;
}

double mcmcmle::getAlphaInformation() {
	if (this->sampleSize>0) {
		return this->alphaInformation / this->sampleSize;
	} else {
		std::cerr << "mcmcmle::getAlphaInformation Error: zero sample size." << std::endl;
		return 0.0;
	}
}

double mcmcmle::getAlphaScore() {
	if (this->sampleSize>0) {
		return this->alphaScore / this->sampleSize;
	} else {
		std::cerr << "mcmcmle::getAlphaScore Error: zero sample size." << std::endl;
		return 0.0;
	}
}


void mcmcmle::calculateDerivativeMatrix(bool useCompleteObservedInformation, bool diagonalOnly) {
	#ifndef CUDA
	if (diagonalOnly) {
		for (int i=0; i<THETA_LENGTH; i++) {
			for (int j=0; j<THETA_LENGTH; j++) {
				if (i != j) {
					this->setDerivative(i,j, 0);
				} else {
					double value;
					if (useCompleteObservedInformation) {
						value = 1/this->getInformation(i,i);
					} else {
						value= 1/this->getScoreCovariance(i,i);
					}
					if (value < MIN_DERIVATIVE) {
						this->setDerivative(i,i,MIN_DERIVATIVE);
					} else if  (value > MAX_DERIVATIVE) {
						this->setDerivative(i,i,MAX_DERIVATIVE);
					} else {
						this->setDerivative(i,i,value);
					}
				}
			}
		}
	} else {
		for (int i=0; i<THETA_LENGTH; i++) {
			for (int j=0; j<THETA_LENGTH; j++) {
				if (useCompleteObservedInformation) {
					gsl_matrix_set(matrixGSL, i, j, this->getObservedInformation(i,j));
				} else {
					gsl_matrix_set(matrixGSL, i, j, this->getScoreCovariance(i,j));
				}
			}
		}
		int s;
		gsl_linalg_LU_decomp(matrixGSL, luPermutationGSL, &s);
		gsl_linalg_LU_invert(matrixGSL, luPermutationGSL, matrixInverseGSL);
		for (int i=0; i<THETA_LENGTH; i++) {
			for (int j=0; j<THETA_LENGTH; j++) {
				this->setDerivative(i,j, gsl_matrix_get(matrixInverseGSL,i,j));
			}
		}
	}

	#else
	// TODO: This should be a CUDA enabled inversion, since gsl gnu is not compatible with nvcc.
	for (int i=0; i<THETA_LENGTH; i++) {
		for (int j=0; j<THETA_LENGTH; j++) {
			if (i != j) {
				this->setDerivative(i,j, 0);
			} else {
				if (useCompleteObservedInformation) {
						this->setDerivative(i,i, 1/this->getInformation(i,i));
				} else {
					this->setDerivative(i,i, 1/this->getScoreCovariance(i,i));
				}
			}
		}
	}
	#endif
}

double mcmcmle::getDerivative(int i, int j) {
	if ((i >= THETA_LENGTH) | (j >= THETA_LENGTH)) {
		std::cerr << "mcmcmle::getDerivative: index out of bounds " << i << "," << j << std::endl;
	}
	return this->derivativeMatrix[i*THETA_LENGTH + j];
}

void mcmcmle::setDerivative(int i, int j, double value) {
	if ((i >= THETA_LENGTH) | (j >= THETA_LENGTH)) {
		std::cerr << "mcmcmle::getDerivative: index out of bounds " << i << "," << j << std::endl;
	}
	this->derivativeMatrix[i*THETA_LENGTH + j] = value;
}

void mcmcmle::partialNewtonRaphsonStep(double gain) {
	for (int i=0; i<THETA_LENGTH; i++) {
		for (int j=0; j<THETA_LENGTH; j++) {
			if (i < BETA_LENGTH) {
				this->completeData->linkEvaluationFunction->incrementBeta(i, gain * this->getScore(i) * this->getDerivative(i,j));
			} else {
				double proposedGamma = this->completeData->crimpEvaluationFunction->getGamma(i-BETA_LENGTH) + gain * this->getScore(i) * this->getDerivative(i,j);
				if (proposedGamma < MIN_GAMMA) {
					#ifdef PRINT_PROPOSAL_RESULTS
							std::cout << "**Clipping gamma to MIN_GAMMA: Proposed " <<  proposedGamma << std::endl;
					#endif
					proposedGamma = MIN_GAMMA;
				} else if (proposedGamma > MAX_GAMMA){
					#ifdef PRINT_PROPOSAL_RESULTS
							std::cout << "**Clipping gamma to MAX_GAMMA: Proposed " <<  proposedGamma << std::endl;
					#endif
					proposedGamma = MAX_GAMMA;
				}
				this->completeData->crimpEvaluationFunction->incrementGamma(i-BETA_LENGTH, proposedGamma);
			}
		}
	}
}

void mcmcmle::partialSingleCoordinateNewtonRaphsonStep(double gain) {
	for (int i=0; i<BETA_LENGTH; i++) {
		this->completeData->linkEvaluationFunction->incrementBeta(i, gain * this->getScore(i) * this->getDerivative(i,i) );
	}

	for (int i=0; i<GAMMA_LENGTH; i++) {
		double proposedGamma = this->completeData->crimpEvaluationFunction->getGamma(i-BETA_LENGTH) +
				gain * this->getGammaScore(i) * this->getDerivative(i+BETA_LENGTH,i+BETA_LENGTH);
		if (proposedGamma < MIN_GAMMA) {
			#ifdef PRINT_PROPOSAL_RESULTS
			std::cout << "**Clipping gamma to MIN_GAMMA: Proposed " <<  proposedGamma << std::endl;
			#endif
			proposedGamma = MIN_GAMMA;
		} else if (proposedGamma > MAX_GAMMA){
			#ifdef PRINT_PROPOSAL_RESULTS
			std::cout << "**Clipping gamma to MAX_GAMMA: Proposed " <<  proposedGamma << std::endl;
			#endif
			proposedGamma = MAX_GAMMA;
		}
		this->completeData->crimpEvaluationFunction->setGamma(i, proposedGamma);
	}
}

void mcmcmle::executePhaseOne(bool singleCoordinate, bool useCompleteInformation,
		bool diagonalOnly, int burns, int sampleSpacing, int samples,
		double gain, bool verbose) {
	this->resetLikelihoodStatistics(false);
	if (verbose) {
		std::cout << "Executing Phase One." << std::endl << ">>Initializing:";
		std::cout << ">>Burning the chain " << burns << " steps." << std::endl;
	}
	for (int i=burns/10; i<=burns; i+=(burns/10)) {
		this->burnChain(burns/10);
		if (verbose) {
			std::cout << "  *Completed " << i << " steps." << std::endl;
		}

	}
	if (verbose) {
		std::cout << ">>Sampling " << samples << " draws from chain distribution with sampling interval " << sampleSpacing << "." << std::endl;
	}
	this->estimateMCMC(sampleSpacing, samples, true);
	if (verbose) {
		std::cout << ">>Draws complete. Estimating derivative matrix." << std::endl;
	}
	this->calculateDerivativeMatrix(useCompleteInformation, diagonalOnly);
	if (verbose) {
		std::cout << ">>Estimation complete. Checking validity of derivative matrix." << std::endl;
	}
	bool valid = true;
	for (int i=0; i<THETA_LENGTH; i++) {
		for (int j=0; j<THETA_LENGTH; j++) {
			if ( (this->getDerivative(i,j) <= DERIV_MAX) & (this->getDerivative(i,j) <= DERIV_MAX) ) {
				// Ok
			} else {
				valid=false;
			}
		}
		if (this->getDerivative(i,i) <= 0) {
			valid=false;
		}
	}
	if (!valid) {
		std::cerr << "Error: The derivative matrix is not valid. Try using diagonal only, or use the score covariance." << std::endl;
	}
	if (verbose) {
		if (valid) {
			std::cout << ">>Derivative matrix is valid." << std::endl;
		} else {
			std::cout << ">>Derivative matrix is NOT valid! Try other settings, or modify your provisional estimate." << std::endl;
		}
		std::cout << "Making a partially estimated, partial single coordinate NR step with gain " << gain << std::endl;
		this->partialSingleCoordinateNewtonRaphsonStep(gain);
		this->print(true);
	}
}

void mcmcmle::executePhaseTwoSubphase(int iterationsMin, int iterationsMax, double gain,
		bool singleCoordinate, int sampleSpacing, int samples, bool verbose){
	if (verbose) {
		std::cout << "Executing a Phase Two Subphase." << std::endl;
		this->print(false);
	}
	// Reset the subphase variables
	for (int i=0; i<THETA_LENGTH; i++) {
		this->subphaseAutocorrScore[i] = 0;
		this->subphasePreviousScore[i]=0;
		this->subphaseSumTheta[i]=0;
		this->subphaseN = 0;
	}
	this->subphaseSumAlpha = 0;
	this->mcmcProposalsMadeInPhase=0;
	this->mcmcProposalsAcceptedInPhase=0;
	bool autocorrelationPositive = true;
	while ( (autocorrelationPositive | (this->subphaseN < iterationsMin))
			& (this->subphaseN < iterationsMax)) {
		this->subphaseN++;
		if (verbose) {
			std::cout << ">>Executing step " << this->subphaseN << std::endl;
		}
		for (int i=0; i<BETA_LENGTH; i++) {
			this->subphasePreviousScore[i]=this->getBetaScore(i);
		}
		for (int i=0; i<GAMMA_LENGTH; i++) {
			this->subphasePreviousScore[i+BETA_LENGTH]=this->getGammaScore(i);
		}
		this->estimateMCMC(sampleSpacing, samples, false);
		if (singleCoordinate) {
			this->partialSingleCoordinateNewtonRaphsonStep(gain);
		} else {
			this->partialNewtonRaphsonStep(gain);
		}
		// Reset the alpha rate
		this->setAlpha( ((double)this->chainLengthSum / (double)this->sampleSize) / (double)this->actors );
		this->subphaseSumAlpha += this->getAlpha();
		bool allAutocorrelationsNegative = true;
		for (int i=0; i<BETA_LENGTH; i++) {
			this->subphaseSumTheta[i] += this->getBeta(i);
			// Slight modification, attenuates the older observations.
			this->subphaseAutocorrScore[i] *= .9;
			this->subphaseAutocorrScore[i] += .1 * this->subphasePreviousScore[i] * this->getBetaScore(i);
			if (this->subphaseAutocorrScore[i] > 0) {
				allAutocorrelationsNegative = false;
			}
		}
		for (int i=0; i<GAMMA_LENGTH; i++) {
			this->subphaseSumTheta[i+BETA_LENGTH] += this->getGamma(i);
			// Slight modification, attenuates the older observations.
			this->subphaseAutocorrScore[i+BETA_LENGTH] *= .9;
			this->subphaseAutocorrScore[i+BETA_LENGTH] += .1 * this->subphasePreviousScore[i+BETA_LENGTH] * this->getGammaScore(i);
			if (this->subphaseAutocorrScore[i] > 0) {
				allAutocorrelationsNegative = false;
			}
		}
		if (verbose) {
			std::cout << "  >> Avg Thetas:" ;
			for (int i=0; i<THETA_LENGTH; i++) {
				std::cout << " " << ( this->subphaseSumTheta[i] /(double)this->subphaseN );
			}
			std::cout << std::endl << "  >> Avg Alpha: " << (this->subphaseSumAlpha /(double)this->subphaseN)
				<< std::endl << "  >> Scores:" ;
			for (int i=0; i<BETA_LENGTH; i++) {
				std::cout << " " << this->getBetaScore(i);
			}
			for (int i=0; i<GAMMA_LENGTH; i++) {
				std::cout << " " << this->getBetaScore(i);
			}
			std::cout << std::endl <<  "  >> LogLike = " << this->getLogLikelihood() << std::endl << "  >> AC:" ;
			for (int i=0; i<BETA_LENGTH; i++) {
				std::cout << " " << subphaseAutocorrScore[i];
			}
			for (int i=0; i<GAMMA_LENGTH; i++) {
				std::cout << " " << subphaseAutocorrScore[i];
			}
			std::cout << std::endl << " >> Acceptance ratio: " << (100 * (double)this->mcmcProposalsAcceptedInPhase / (double)this->mcmcProposalsMadeInPhase) << "%" << std::endl;
		}
		if ( (allAutocorrelationsNegative) & (this->subphaseN > 1)) {
			autocorrelationPositive = false;
			if (verbose) {
				std::cout << ">>Autocorrelation criteria met." << std::endl;
			}
		}
		bool thetaValid = true;
		for (int i=0; i<THETA_LENGTH; i++) {
			if (this->subphaseSumTheta[i] != this->subphaseSumTheta[i]) {
				std::cerr << "Theta exceeded numerical limits. Try less sensitive parameterizations for R-M." << std::endl;
				thetaValid = false;
			}
		}
		if (!thetaValid) {
			break;
		}
	}
	this->setAlpha( (this->subphaseSumAlpha /(double)this->subphaseN) );
	for (int i=0; i<BETA_LENGTH; i++) {
		this->setBeta(i, (this->subphaseSumTheta[i] /(double)this->subphaseN) );
	}
	for (int i=0; i<GAMMA_LENGTH; i++) {
		this->setGamma(i, (this->subphaseSumTheta[i+BETA_LENGTH] /(double)this->subphaseN) );
	}
	if (verbose) {
		std::cout << ">>Subphase completed." << std::endl;
		this->print(false);
	}
}

void mcmcmle::executePhaseThree(int sampleSpacing, int samples, bool verbose) {
	if (verbose) {
		std::cout << ">>Executing Phase three." << std::endl;
	}
	this->resetLikelihoodStatistics(false);
	this->estimateMCMC(sampleSpacing,samples, true);
	this->calculateConvergenceStatistics();
	this->calculateStandardErrorsSquared();
	if (verbose) {
		this->print(true);
	}
}

void mcmcmle::calculateConvergenceStatistics() {
	//Calculate Convergence Statistics
	for (int i=0; i<THETA_LENGTH; i++) {
		for (int j=0; j<THETA_LENGTH; j++) {
			gsl_matrix_set(matrixGSL, i, j, this->getScoreCovariance(i,j));
		}
	}
	int s;
	gsl_linalg_LU_decomp(matrixGSL, luPermutationGSL, &s);
	gsl_linalg_LU_invert(matrixGSL, luPermutationGSL, matrixInverseGSL);

	for (int i=0; i<THETA_LENGTH; i++) {
		double value = 0;
		for (int j=0; j<THETA_LENGTH; j++) {
			value +=  this->getScore(i) * this->getScore(j) * gsl_matrix_get(matrixInverseGSL,i,j);
		}
		value = sqrt(this->sampleSize*value);
		if (value != value) {
			// Value too small
			value = 0.0;
		}
		this->setConvergenceStatistic(i, value);
	}
}

void mcmcmle::estimateProfileLogLikelihood(int betaIndex, double betaMin,
		double betaMax, int samples, double* betaFixed, double* profileLogLikelihood,
		int MCMCsamples, int sampleSpacing) {
	//TODO: Debug profile log likelihood for Gamma parameters
	double oldBeta = this->getBeta(betaIndex);
	double range = betaMax - betaMin;
	if (range <=0) {
		std::cerr << "Bad range for profile log likelihood! Min must be < Max." << std::endl;
	} else {
		for (int i=0; i<samples; i++) {
			profileLogLikelihood[i]=0;
			betaFixed[i] = betaMin + (double)i * range / (double)(samples-1);
			this->setBeta(betaIndex, betaFixed[i]);
			this->burnChain(500);
			this->estimateMCMC(MCMCsamples, sampleSpacing, true);
			profileLogLikelihood[i] = this->getLogLikelihood();
//			std::cout << "Beta " << i << " : " << betaFixed[i] << " -->" <<
//					profileLogLikelihood[i] << std::endl;
		}
	}
	this->setBeta(betaIndex, oldBeta);
}

int mcmcmle::getActors() {
	return this->actors;
}

double mcmcmle::getAverageSubphaseTheta(int i) {
	return ( this->subphaseSumTheta[i] / (double)(this->subphaseN) );
}

double mcmcmle::getSubphaseScoreAutocorr(int i) {
	return this->subphaseAutocorrScore[i];
}

double mcmcmle::getProposalAcceptanceRatio() {
	double returnValue = 0;
	if ((this->mcmcProposalsAcceptedInPhase)) {
		returnValue = (double)(this->mcmcProposalsAcceptedInPhase) / (double)(this->mcmcProposalsMadeInPhase);
	} else {
		std::cerr << "No proposals have been drawn!" << std::endl;
	}
	return returnValue;
}

void mcmcmle::print() {
	this->print(false);
}

void mcmcmle::print(bool verbose) {
	std::cout << "MCMC-MLE Results" << std::endl;
	if (this->sampleSize == 0) {
		std::cout << "**No samples drawn yet." << std::endl;
		std::cout << ">>Theta:      " ;
		for (int i=0; i<BETA_LENGTH; i++) {
			printf(" %+9.3f|", this->getBeta(i));
		}
		for (int i=0; i<GAMMA_LENGTH; i++) {
			printf(" %+9.3f|", this->getGamma(i));
		}
		std::cout << std::endl;
	} else {
		if (this->sampleSize >1) {
			std::cout << "**MCMC Accept.: " << this->getProposalAcceptanceRatio() << std::endl;
			std::cout << "**MCMC Samples: " << this->sampleSize << std::endl;
		} else {
			std::cout << "**Single complete data sample used." << std::endl;
		}
		std::cout << ">>Mean LogLike of samples = " << this->completeData->getLogLikelihood() << std::endl;
		std::cout << ">>Theta:      " ;
		for (int i=0; i<BETA_LENGTH; i++) {
			printf(" %+9.3f|", this->getBeta(i));
		}
		for (int i=0; i<GAMMA_LENGTH; i++) {
			printf(" %+9.3f|", this->getGamma(i));
		}
		std::cout << std::endl;
		std::cout << ">>Rate*:      ";
		printf(" %+9.3f|", this->getAlpha());
		printf(" %+9.3f|", this->getAlphaScore());
		printf(" %+9.3f|", this->getAlphaInformation());
		std::cout << std::endl;
		std::cout << ">>Score:      ";
		for (int i=0; i<THETA_LENGTH; i++) {
				printf(" %+9.3f|", this->getScore(i));
		}
		std::cout << std::endl;
		std::cout << ">>Full NRStep:";
		for (int i=0; i<THETA_LENGTH; i++) {
			double diffTheta = 0.0;
			for (int j=0; j<THETA_LENGTH; j++) {
				diffTheta += this->getScore(j) * this->getDerivative(i,j);
			}
			printf(" %+9.3f|", diffTheta);
		}
		std::cout << std::endl;
		std::cout << ">>S.C. NRStep:";
		for (int i=0; i<THETA_LENGTH; i++) {
			printf(" %+9.3f|", this->getScore(i) * this->getDerivative(i,i));
		}
		std::cout << std::endl;
		std::cout << ">>Std. Errors:";
		for (int i=0; i<THETA_LENGTH; i++) {
			printf(" %+9.3f|", sqrt(this->getStandardErrorSquared(i,i)) );
		}
		std::cout << std::endl;
		std::cout << ">>Convergence:";
		for (int i=0; i<THETA_LENGTH; i++) {
			printf(" %+9.3f|", sqrt(this->getConvergenceStatistic(i)) );
		}
		std::cout << std::endl;
		if (verbose) {
			std::cout << ">>Information: " << std::endl;
			for (int i=0; i<THETA_LENGTH; i++) {
				std::cout << "         ";
				for (int j=0; j<THETA_LENGTH; j++) {
					printf(" %+9.3f", this->getInformation(i,j));
				}
				std::cout << std::endl;
			}
			std::cout << std::endl;
			std::cout << ">>Obs. Information: " << std::endl;
			for (int i=0; i<THETA_LENGTH; i++) {
				std::cout << "         ";
				for (int j=0; j<THETA_LENGTH; j++) {
					printf(" %+9.3f", this->getObservedInformation(i,j));
				}
				std::cout << std::endl;
			}
			std::cout << ">>Score Covariance: " << std::endl;
			for (int i=0; i<THETA_LENGTH; i++) {
				std::cout << "         ";
				for (int j=0; j<THETA_LENGTH; j++) {
					printf(" %+9.3f", this->getScoreCovariance(i,j));
				}
				std::cout << std::endl;
			}
			std::cout << ">>Derivative: " << std::endl;
			for (int i=0; i<THETA_LENGTH; i++) {
				std::cout << "         ";
				for (int j=0; j<THETA_LENGTH; j++) {
					printf(" %+9.3f", this->getDerivative(i,j));
				}
				std::cout << std::endl;
			}
			std::cout << ">>Cov{ThetaHat}: " << std::endl;
			for (int i=0; i<THETA_LENGTH; i++) {
				std::cout << "         ";
				for (int j=0; j<THETA_LENGTH; j++) {
					printf(" %+9.3f", this->getStandardErrorSquared(i,j));
				}
				std::cout << std::endl;
			}
		} else {
			std::cout << ">>Information:";
			for (int i=0; i<THETA_LENGTH; i++) {
				printf(" %+9.3f|", this->getInformation(i,i));
			}
			std::cout << std::endl;
			std::cout << ">>Obs. Info.: ";
			for (int i=0; i<THETA_LENGTH; i++) {
				printf(" %+9.3f|", this->getObservedInformation(i,i));
			}
			std::cout << std::endl;
			std::cout << ">>Score Cov.: ";
			for (int i=0; i<THETA_LENGTH; i++) {
				printf(" %+9.3f|", this->getScoreCovariance(i,i));
			}
			std::cout << std::endl;
			std::cout << ">>Derivative: ";
			for (int i=0; i<THETA_LENGTH; i++) {
				printf(" %+9.3f|", this->getDerivative(i,i));
			}
			std::cout << std::endl;
		}
	}
}

void mcmcmle::setLogLikelihoodSum(double value) {
	this->logLikelihood = value;
}

void mcmcmle::setAlphaScoreSum(double value) {
	this->alphaScore = value;
}

void mcmcmle::setAlphaInformationSum(double value) {
	this->alphaInformation = value;
}

void mcmcmle::incrementAlphaScoreSum(double value) {
	this->alphaScore += value;
}

void mcmcmle::incrementAlphaInformationSum(double value) {
	this->alphaInformation += value;
}


void mcmcmle::setBetaScoreSum(int k, double value) {
	if ( k < BETA_LENGTH) {
		this->betaScore[k] = value;
	} else {
		std::cerr << "mcmcmle::setBetaScoreSum: index out of bounds " << k << std::endl;
	}
}

void mcmcmle::setGammaScoreSum(int k, double value) {
	if ( k < GAMMA_LENGTH) {
		this->gammaScore[k] = value;
	} else {
		std::cerr << "mcmcmle::setGammaScoreSum: index out of bounds " << k << std::endl;
	}
}

void mcmcmle::setScoreSum(int k, double value) {
	if ( k < BETA_LENGTH) {
		this->setBetaScoreSum(k,value);
	} else {
		this->setGammaScoreSum(k-BETA_LENGTH,value);
	}
}

void mcmcmle::setBetaInformationSum(int j, int k, double value) {
	if ((j < BETA_LENGTH) & (k < BETA_LENGTH)) {
		this->betaInformation[j*BETA_LENGTH+k] = value;
	} else {
		std::cerr << "mcmcmle::setBetaInformationSum: index out of bounds " << j << "," << k << std::endl;
	}
}

void mcmcmle::setGammaInformationSum(int j, int k, double value) {
	if ((j < GAMMA_LENGTH) & (k < GAMMA_LENGTH)) {
		this->gammaInformation[j*GAMMA_LENGTH+k] = value;
	} else {
		std::cerr << "mcmcmle::setGammaInformationSum: index out of bounds " << j << "," << k << std::endl;
	}
}

void mcmcmle::setStandardErrorSquared(int j, int k, double value) {
	if ((j < THETA_LENGTH) & (k < THETA_LENGTH)) {
		this->standardErrorsSquared[j*THETA_LENGTH+k] = value;
	} else {
		std::cerr << "mcmcmle::setStandardErrorSquared: index out of bounds " << j << "," << k << std::endl;
	}
}

double mcmcmle::getStandardErrorSquared(int j, int k) {
	if ((j < THETA_LENGTH) & (k < THETA_LENGTH)) {
		return this->standardErrorsSquared[j*THETA_LENGTH+k];
	} else {
		std::cerr << "mcmcmle::getStandardErrorSquared: index out of bounds " << j << "," << k << std::endl;
		return 0.0;
	}
}

void mcmcmle::setScoreSquaredSum(int j, int k, double value) {
	if ((j < THETA_LENGTH) & (k < THETA_LENGTH)) {
		this->scoreSquaredSum[j*THETA_LENGTH+k] = value;
	} else {
		std::cerr << "mcmcmle::setScoreSquaredSum: index out of bounds " << j << "," << k << std::endl;
	}
}

double mcmcmle::getScoreSquaredSum(int j, int k) {
	if ((j < THETA_LENGTH) & (k < THETA_LENGTH)) {
		return this->scoreSquaredSum[j*THETA_LENGTH+k];
	} else {
		std::cerr << "mcmcmle::getScoreSquaredSum: index out of bounds " << j << "," << k << std::endl;
		return 0.0;
	}
}

void mcmcmle::incrementLogLikelihoodSum(double value) {
	this->logLikelihood += value;
}

void mcmcmle::incrementBetaScoreSum(int k, double value) {
	if ( k < BETA_LENGTH) {
		this->betaScore[k] += value;
	} else {
		std::cerr << "mcmcmle::incrementBetaScoreSum: index out of bounds " << k << std::endl;
	}
}

void mcmcmle::incrementGammaScoreSum(int k, double value) {
	if ( k < GAMMA_LENGTH) {
		this->gammaScore[k] += value;
	} else {
		std::cerr << "mcmcmle::incrementGammaScoreSum: index out of bounds " << k << std::endl;
	}
}

void mcmcmle::incrementBetaInformationSum(int j, int k, double value) {
	if ((j < BETA_LENGTH) & (k < BETA_LENGTH)) {
		this->betaInformation[j*BETA_LENGTH+k] += value;
	} else {
		std::cerr << "mcmcmle::incrementBetaInformationSum: index out of bounds " << j << "," << k << std::endl;
	}
}

void mcmcmle::incrementGammaInformationSum(int j, int k, double value) {
	if ((j < GAMMA_LENGTH) & (k < GAMMA_LENGTH)) {
		this->gammaInformation[j*GAMMA_LENGTH+k] += value;
	} else {
		std::cerr << "mcmcmle::incrementGammaInformationSum: index out of bounds " << j << "," << k << std::endl;
	}
}

void mcmcmle::incrementScoreSquaredSum(int j, int k, double value) {
	if ((j < THETA_LENGTH) & (k < THETA_LENGTH)) {
		this->scoreSquaredSum[j*THETA_LENGTH+k] += value;
	} else {
		std::cerr << "mcmcmle::incrementScoreSquaredSum: index out of bounds " << j << "," << k << std::endl;
	}
}

double mcmcmle::getConvergenceStatistic(int i) {
	if ( i < THETA_LENGTH) {
		return this->convergenceStatistics[i];
	} else {
		std::cerr << "mcmcmle::getConvergenceStatistic: index out of bounds " << i << std::endl;
		return 0.0;
	}
}

void mcmcmle::setConvergenceStatistic(int i, double value) {
	if ( i < THETA_LENGTH) {
		this->convergenceStatistics[i] = value;
	} else {
		std::cerr << "mcmcmle::getConvergenceStatistic: index out of bounds " << i << std::endl;
	}
}

void mcmcmle::resetLikelihoodStatistics(bool scoreOnly) {
	this->sampleSize = 0;
	this->mcmcProposalsAcceptedInPhase = 0;
	this->mcmcProposalsMadeInPhase = 0;
	this->setLogLikelihoodSum(0);
	for (int i=0; i<THETA_LENGTH; i++) {
		if (i<BETA_LENGTH) {
			this->setBetaScoreSum(i, 0);
		} else {
			this->setGammaScoreSum(i-BETA_LENGTH,0);
		}
		for (int j=0; j<THETA_LENGTH; j++) {
			if (!scoreOnly) {
				this->setInformationSum(i,j,0);
				this->setDerivative(i,j,0);
				this->setStandardErrorSquared(i,j,0);
			}
			this->setScoreSquaredSum(i,j,0);
		}
	}
}

void mcmcmle::setInformationSum(int a, int b, double value) {
	if ((a < BETA_LENGTH) & (b < BETA_LENGTH)) {
		 this->setBetaInformationSum(a, b, value);
	} else if ((a >= BETA_LENGTH) & (b >= BETA_LENGTH)){
		this->setGammaInformationSum(a-BETA_LENGTH, b-BETA_LENGTH, value);
	}
}

void mcmcmle::incrementScoreSum(int k, double value) {
	if (k < BETA_LENGTH) {
		this->incrementBetaScoreSum(k, value);
	} else {
		this->incrementGammaScoreSum(k-BETA_LENGTH, value);
	}
}

void mcmcmle::incrementInformationSum(int a, int b, double value) {
	if ((a < BETA_LENGTH) & (b < BETA_LENGTH)) {
		 this->incrementBetaInformationSum(a, b, value);
	} else if ((a >= BETA_LENGTH) & (b >= BETA_LENGTH)){
		this->incrementGammaInformationSum(a-BETA_LENGTH, b-BETA_LENGTH, value);
	}
}

}
